#include "target.h"
#include "padl_sysctrl.h"

#include "../config/phal_boot_config.h"
#include "phal_boot.h"


static void phal_boot_configurePLL(void)
{
	if (padl_sysctrl_IS_PLL_CONNECTED())
  	{
		// Enable PLL, disconnected
		padl_sysctrl_ENABLE_PLL();
  	}

  	// Disable PLL, disconnected
  	padl_sysctrl_DISABLE_PLL();

  	// Enable main OSC
  	padl_sysctrl_ENABLE_MAIN_OSCILATOR();
  	// Wait until main OSC is usable
  	while(!padl_sysctrl_IS_MAIN_OSCILATOR_READY());

	// Select main OSC, as the PLL clock source
  	padl_sysctrl_SET_PLL_CLOCK_SOURCE(PLL_SOURCE_MAIN_OSC);

  	padl_sysctrl_SET_PLL_CONFIGURATION(PLL_MValue, PLL_NValue);

  	// Enable PLL, disconnected
	padl_sysctrl_ENABLE_PLL();

  	padl_sysctrl_SET_CPU_CLOCK_DIVISOR(CCLKCFG_Val);
  	padl_sysctrl_SET_USB_CLOCK_DIVISOR(USBCLKCFG_Val);

  	// Check lock bit status
  	while (!padl_sysctrl_IS_PLL_LOCKED());

  	// Wait until reach target PLL clock
  	while ((padl_sysctrl_GET_ACTUAL_PLL_MUL() != PLL_MValue)
  			&& ( padl_sysctrl_GET_ACTUAL_PLL_DIV() != PLL_NValue));

	padl_sysctrl_CONNECT_PLL();

  	// Check connect bit status
	while (!padl_sysctrl_IS_PLL_CONNECTED());
}

static void phal_boot_configureMAM(void)
{
	U32  systemFreq;

    systemFreq = padl_sysctrl_GET_SYSTEM_FRQ();			        /* Get the current core clock frequency                     */

    padl_sysctrl_SYSCTRL->MAMCR    = 0;                                               /* Disable MAM functionality                                */

    if (systemFreq < 20000000) {                                /* Compare current clock frequency with MAM modes           */
    	padl_sysctrl_SYSCTRL->MAMTIM = 1;                                             /* Set MAM fetch cycles to 1 processor clock in duration    */
    }

    if (systemFreq < 40000000) {
    	padl_sysctrl_SYSCTRL->MAMTIM = 2;                                             /* Set MAM fetch cycles to 2 processor clock in duration    */
    }

    if (systemFreq >= 40000000) {
    	padl_sysctrl_SYSCTRL->MAMTIM = 3;                                             /* Set MAM fetch cycles to 3 processor clock in duration    */
    }

    padl_sysctrl_SYSCTRL->MAMCR = 2;                                                  /* Enable full MAM functionality                            */
}

void phal_boot_start(void)
{
	phal_boot_configurePLL();
	phal_boot_configureMAM();
}

void phal_boot_finish(void)
{	
}
